{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##  Multi-Contact dynamics for biped walking (Talos legs)\n",
    "In this example, we describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
    "$$\n",
    "\\left[\\begin{matrix}\n",
    " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
    " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
    "\\end{matrix}\\right]\n",
    "\\left[\\begin{matrix}\n",
    " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
    "\\end{matrix}\\right]\n",
    " = \n",
    "\\left[\\begin{matrix}\n",
    "  \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
    "  -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
    "\\end{matrix}\\right]$$.\n",
    "\n",
    "\n",
    "Base on a predefined walking gait, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
    "\n",
    "\n",
    "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
    "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
    "\t\t\t\t  \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
    "\\end{Bmatrix} =\n",
    "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
    "subject to\n",
    "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
    "\n",
    "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
    "\n",
    "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
    "\n",
    "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
    "\n",
    "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
    "\n",
    "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
    "\n",
    "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "SimpleBipedGaitProblem class builds action models for each locomotion phase:\n",
    "  - createSwingFootModel: defines an action model for the swing phase\n",
    "  - createFootSwitchModel: defines an action model for switch knots between phases\n",
    "  \n",
    "Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
    "\n",
    "Now let's create a walking OC problem for the Talos legs."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import crocoddyl\n",
    "import pinocchio\n",
    "import numpy as np\n",
    "import example_robot_data\n",
    "from crocoddyl.utils.biped import SimpleBipedGaitProblem\n",
    "\n",
    "# Creating the lower-body part of Talos\n",
    "talos_legs = example_robot_data.loadTalosLegs()\n",
    "\n",
    "# Setting up the 3d walking problem\n",
    "rightFoot = 'right_sole_link'\n",
    "leftFoot = 'left_sole_link'\n",
    "gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)\n",
    "\n",
    "\n",
    "# Create the initial state\n",
    "q0 = talos_legs.q0.copy()\n",
    "v0 = pinocchio.utils.zero(talos_legs.model.nv)\n",
    "x0 = np.concatenate([q0,v0])\n",
    "\n",
    "\n",
    "# Creating the walking problem\n",
    "stepLength = 0.6 # meters\n",
    "stepHeight = 0.1 # meters\n",
    "timeStep = 0.0375 # seconds\n",
    "stepKnots = 20\n",
    "supportKnots = 10\n",
    "problem = gait.createWalkingProblem(x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots)\n",
    "\n",
    "\n",
    "# Solving the 3d walking problem using Feasibility-prone DDP\n",
    "ddp = crocoddyl.SolverFDDP(problem)\n",
    "\n",
    "# Using the meshcat displayer, you could enable gepetto viewer for nicer view\n",
    "# display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, frameNames=[rightFoot, leftFoot])\n",
    "display = crocoddyl.MeshcatDisplay(talos_legs, 4, 4, False)\n",
    "ddp.setCallbacks([crocoddyl.CallbackLogger(),\n",
    "                  crocoddyl.CallbackVerbose(),\n",
    "                  crocoddyl.CallbackDisplay(display)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Emdebbed meshcat in this cell\n",
    "display.robot.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Solve the optimal control problem\n",
    "ddp.th_stop = 1e-9\n",
    "init_xs = [talos_legs.model.defaultState] * (problem.T + 1)\n",
    "init_us = []\n",
    "maxiter = 1000\n",
    "regInit = 0.1\n",
    "ddp.solve(init_xs, init_us, maxiter, False, regInit)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the following commands we can plot \n",
    " - the state and control trajectories, and\n",
    " - the DDP performance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "# Plotting the solution and the DDP convergence\n",
    "log = ddp.getCallbacks()[0]\n",
    "crocoddyl.plotOCSolution(log.xs, log.us)\n",
    "crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally we can visualize the solution using gepetto-viewer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualization of the DDP solution in gepetto-viewer\n",
    "rate = 2.\n",
    "display.displayFromSolver(ddp, rate)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## VI. Understanding the walking problem\n",
    "\n",
    "In this problem we pre-defined a 20 and 10 knots for the step and double support phases with $dt=$3.75e-2, repectively. \n",
    "\n",
    " 1. Could you tell us how much is the foot step and double support duration?\n",
    " 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
    " 3. What happens when do we change the number of step knots (e.g. 10)?\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
